#include "camera.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <asm/types.h>
#include <linux/v4l2-subdev.h>

void Cam_PrintCapability(Cam_Device_t *dev, struct v4l2_capability *cap)
{
    struct v4l2_format fmt;

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    ioctl(dev->fd, VIDIOC_G_FMT, &fmt);
        printf("------- VIDIOC_G_FMT ----\n");
        printf("with=%d, heigh=%d, format %c%c%c%c\n",
                fmt.fmt.pix.width,
                fmt.fmt.pix.height,
                fmt.fmt.pix.pixelformat & 0xFF,
               (fmt.fmt.pix.pixelformat >> 8) & 0xFF,
               (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
               (fmt.fmt.pix.pixelformat >> 24) & 0xFF);

    printf("------- VIDIOC_QUERYCAP ----\n");
    printf("  driver: %s\n", cap->driver);
    printf("  card: %s\n", cap->card);
    printf("  bus_info: %s\n", cap->bus_info);
    printf("  version: %d.%d.%d\n",
           (cap->version >> 16) & 0xff,
           (cap->version >> 8) & 0xff,
           (cap->version & 0xff));
    printf("  capabilities: %08X\n", cap->capabilities);

    if (cap->capabilities & V4L2_CAP_VIDEO_CAPTURE)
        printf("        Video Capture\n");
    if (cap->capabilities & V4L2_CAP_VIDEO_OUTPUT)
        printf("        Video Output\n");
    if (cap->capabilities & V4L2_CAP_VIDEO_OVERLAY)
        printf("        Video Overly\n");
    if (cap->capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)
        printf("        Video Capture Mplane\n");
    if (cap->capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE)
        printf("        Video Output Mplane\n");
    if (cap->capabilities & V4L2_CAP_READWRITE)
        printf("        Read / Write\n");
    if (cap->capabilities & V4L2_CAP_STREAMING)
        printf("        Streaming\n");
    printf("\n");

}

int Cam_SetFmt(int fd, Cam_Config_t *cfg)
{
    struct v4l2_format fmt;

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    fmt.fmt.pix.pixelformat = cfg->fmt;
    fmt.fmt.pix.width = cfg->width;  //640
    fmt.fmt.pix.height = cfg->height;  //640
    if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
        printf("VIDIOC_S_FMT failed - [%d]!\n", errno);
        return -1;
    }
    printf("VIDIOC_S_FMT succeed!\n");

    printf("width %d, height %d, size %d, bytesperline %d, format %c%c%c%c\n\n",
           fmt.fmt.pix.width, fmt.fmt.pix.height, fmt.fmt.pix.sizeimage,
           fmt.fmt.pix.bytesperline,
           fmt.fmt.pix.pixelformat & 0xFF,
           (fmt.fmt.pix.pixelformat >> 8) & 0xFF,
           (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
           (fmt.fmt.pix.pixelformat >> 24) & 0xFF);

    return fmt.fmt.pix.sizeimage;
}

int Cam_EnqueueBuf(Cam_Device_t *device, int index)
{
    struct v4l2_buffer buf;
    struct v4l2_plane plane;
    memset(&buf, 0, sizeof(buf));
    memset(&plane, 0, sizeof(plane));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = index;
    buf.m.planes = &plane;
    buf.length = 1;

    if (ioctl(device->fd, VIDIOC_QBUF, &buf) < 0) {
        printf("VIDIOC_QBUF failed!\n\n");
        return -1;
    }
    return 0;
}

int Cam_DequeueBuf(Cam_Device_t *device)
{
    struct v4l2_buffer buf;
    struct v4l2_plane plane;
    memset(&buf, 0, sizeof(buf));
    memset(&plane, 0, sizeof(plane));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = 0;
    buf.m.planes = &plane;
    buf.length = 1;

    if (ioctl(device->fd, VIDIOC_DQBUF, &buf) < 0) {
        printf("VIDIOC_QBUF failed!\n\n");
        return -1;
    }
    return buf.index;
}

int Camera_StreamOn(Cam_Device_t *dev)
{
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    if (ioctl(dev->fd, VIDIOC_STREAMON, &type) == -1) {
        printf("VIDIOC_STREAMON failed!\n\n");
        return -1;
    }
    printf("VIDIOC_STREAMON succeed!\n\n");
    return 0;
}

int Camera_StreamOff(Cam_Device_t *dev)
{
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) == -1) {
        printf("VIDIOC_STREAMOFF failed!\n\n");
        return -1;
    }
    printf("VIDIOC_STREAMOFF succeed!\n\n");
    return 0;
}

Cam_Device_t *Camera_Init(Cam_Config_t *cfg)
{
    int fd, i;
    int frame_size;
    struct v4l2_capability cap;
    struct v4l2_requestbuffers req;
    struct v4l2_buffer buf;
    struct v4l2_exportbuffer expbuf;
    struct v4l2_plane plane;
    Cam_Device_t *device;

    device = (Cam_Device_t*)malloc(sizeof(Cam_Device_t) + sizeof(Cam_Buffer_t) * cfg->buf_num);
    if (!device)
    {
        return NULL;
    }

    device->buf_num = cfg->buf_num;

    device->fd = open("/dev/video0", O_RDWR | O_CLOEXEC, 0);
    if (device->fd < 0) {
        printf("Cannot open \n\n", "/dev/video0");
        free(device);
        return NULL;
    }

    if (ioctl(device->fd, VIDIOC_QUERYCAP, &cap) < 0) {
        printf("VIDIOC_QUERYCAP failed\n");
        goto fail;
    }

    Cam_PrintCapability(device, &cap);

    frame_size = Cam_SetFmt(device->fd, cfg);
    if (frame_size < 0)
    {
        printf("set format failed\n");
        goto fail;
    }

    //alloc buf
    memset(&req, 0, sizeof(req));
    req.count = cfg->buf_num;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    req.memory = V4L2_MEMORY_MMAP;
    if (ioctl(device->fd, VIDIOC_REQBUFS, &req) == -1) {
        printf("VIDIOC_REQBUFS failed!\n\n");
        goto fail;
    }
    if (cfg->buf_num != req.count) {
        printf("!!! req count = %d\n", req.count);
        goto fail;
    }
    printf("VIDIOC_REQBUFS succeed!\n\n");

    //map buf to user space and export to dmabuf
    for (i = 0; i < cfg->buf_num; i++)
    {
        memset(&buf, 0, sizeof(buf));
        memset(&plane, 0, sizeof(plane));
        memset(&expbuf, 0, sizeof(expbuf));

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        buf.m.planes = &plane;
        buf.length = 1;
        
        if (ioctl(device->fd, VIDIOC_QUERYBUF, &buf) == -1)
        {
            printf("VIDIOC_QUERYBUF failed!\n\n");
        }

        expbuf.type = buf.type;
        expbuf.index = i;
        expbuf.flags = O_CLOEXEC;
        if (ioctl(device->fd, VIDIOC_EXPBUF, &expbuf) == -1)
        {
            printf("VIDIOC_EXPBUF failed!\n\n");
        }

        device->buf[i].fd = expbuf.fd;
        device->buf[i].length = buf.m.planes[0].length;
        device->buf[i].start = mmap(NULL, buf.m.planes[0].length, PROT_READ | PROT_WRITE, MAP_SHARED, device->fd, buf.m.planes[0].m.mem_offset);

    }

    printf("print v4l2 buf info:\n");
    for (i = 0; i < cfg->buf_num; i++)
    {
        printf("buf[%d]:length = %d, dmabu = %d\n", i , device->buf[i].length, device->buf[i].fd);
    }
    return device;
fail:
    free(device);
    close(fd);
    return NULL;
}

void Camera_DeInit(Cam_Device_t *device)
{
    close(device->fd);
    free(device);
}

